﻿#include "route_dispatcher.h"

#include <algorithm>
#include <cmath>
#include <limits>
#include <QStringList>
#include <QSet>
#include <QTimer>
#include <QtCore/QMetaObject>
#include <QtConcurrent/QtConcurrent>

#include "application/map/area/area.h"
#include "application/map/lift/lift_manager.h"
#include "application/map/auto_door/auto_door_manager.h"
#include "application/map/third_area/third_area_manager.h"
#include "application/map/third_area/third_block_area.h"
#include "application/pathfinding/pathfinding_graph.h"
#include "application/pathfinding/temporal_graph.h"
#include "concurrent_combination_solver.h"

namespace application {
	namespace dispatcher {

		RouteDispatcher::RouteDispatcher(
			const QString& mapName, int floor,
			std::shared_ptr<vehicle::VehicleService> vehicleService,
			std::shared_ptr<MapApplicationService> mapService,
			std::shared_ptr<kernel::ILogger> logger, QObject* parent)
			: QObject(parent),
			m_mapName(mapName),
			m_floor(floor),
			m_vehicleService(std::move(vehicleService)),
			m_mapService(std::move(mapService)),
			m_logger(std::move(logger)) {
			connect(&m_concurrentWatcher,
				&QFutureWatcher<ConcurrentCombinationSolver::SolveResult>::finished,
				this, &RouteDispatcher::onConcurrentSolveFinished);
		}

		void RouteDispatcher::setDispatchCallback(
			std::function<void(int, const QList<QString>&)> cb) {
			m_dispatchCallback = std::move(cb);
		}

		void RouteDispatcher::init() {
			// Demand-driven: do not start timers by default
			m_solveTimer.setSingleShot(true);
			connect(&m_solveTimer, &QTimer::timeout, this,
				&RouteDispatcher::slotReqPathSolveTimer);
			m_publishTimer.setInterval(200);
			m_publishTimer.setSingleShot(false);
			connect(&m_publishTimer, &QTimer::timeout, this,
				&RouteDispatcher::slotPublishMoveTask);
			if (m_vehicleService) {
				connect(m_vehicleService.get(),
					&application::vehicle::VehicleService::vehicleStatusChanged, this,
					&RouteDispatcher::onVehicleStatusChanged, Qt::QueuedConnection);
			}

			// 导航状态检查定时器（每2秒检查一次）
			m_stateCheckTimer.setInterval(2000);
			m_stateCheckTimer.setSingleShot(false);
			connect(&m_stateCheckTimer, &QTimer::timeout, this,
				&RouteDispatcher::slotCheckNavigationState);

			// Connect area exit signals to resume waiting tasks (all loaded floors)
			if (m_mapService) {
				auto floors = m_mapService->getLoadedFloors();
				for (int floor : floors) {
					auto areaMgr = m_mapService->getAreaManager(floor);
					if (!areaMgr) continue;
					auto ids = areaMgr->getAllAreaIds();
					for (const auto& id : ids) {
						auto area = areaMgr->getArea(id);
						if (area) {
							connect(area.get(), &application::area::Area::areaExited, this,
								[this](int agvId, const QString& areaId) {
								onAreaExited(agvId, areaId);
							});
						}
					}
				}
			}
			stopPublishTimerIfIdle();
		}

		void RouteDispatcher::onVehicleStatusChanged(int agvNumber) {
			auto* agvEntity =
				m_vehicleService ? m_vehicleService->getVehicle(agvNumber) : nullptr;
			if (!agvEntity) return;

			const bool onThisMap =
				QString::fromStdString(agvEntity->getCurrentMap()) == m_mapName;
			const bool hasState =
				std::any_of(m_pending.begin(), m_pending.end(), [agvNumber](const PendingRequest& r) { return r.agvNumber == agvNumber; }) ||
				m_plannedSegments.contains(agvNumber) ||
				m_waitingAgvs.contains(agvNumber) ||
				m_dispatchedPending.contains(agvNumber) ||
				m_lastSegmentGoal.contains(agvNumber) ||
				m_taskGoalCache.contains(agvNumber);
			if (!agvEntity->isOnline() || !onThisMap) {
				if (!hasState) {
					if (m_logger) {
						m_logger->debug(QString("车辆状态变更：AGV %1 不在本地图/离线，但当前调度器无状态，忽略")
							.arg(agvNumber));
					}
					return;
				}
				if (m_logger) {
					m_logger->info(QString("车辆状态变更：清理AGV %1 队列（在线=%2 地图=%3 期望=%4）")
						.arg(agvNumber)
						.arg(agvEntity->isOnline() ? QStringLiteral("true") : QStringLiteral("false"))
						.arg(QString::fromStdString(agvEntity->getCurrentMap()))
						.arg(m_mapName));
				}
				cancelPendingForAgv(agvNumber, QStringLiteral("vehicle offline or map mismatch"));
				// 如果AGV离线或离开地图，也从等待车列表中移除
				m_waitingAgvs.remove(agvNumber);
				m_dispatchedPending.remove(agvNumber);
				stopPublishTimerIfIdle();
				return;
			}

			auto it = m_plannedSegments.find(agvNumber);
			QString currentStation =
				QString::fromStdString(agvEntity->getCurrentStation());
			if (currentStation.isEmpty()) {
				currentStation = QString::fromStdString(agvEntity->getLastStation());
			}

			if (it == m_plannedSegments.end()) {
				if (m_lastSegmentGoal.contains(agvNumber) && !currentStation.isEmpty() &&
					currentStation == m_lastSegmentGoal.value(agvNumber)) {
					QString releasedStation = currentStation;
					m_lastSegmentGoal.remove(agvNumber);
					// 检查是否有等待车需要重新调度
					checkAndRescheduleWaitingAgvs(releasedStation);
				}
				return;
			}

			auto& queue = it.value();
			if (queue.isEmpty()) {
				if (m_lastSegmentGoal.contains(agvNumber) && !currentStation.isEmpty() &&
					currentStation == m_lastSegmentGoal.value(agvNumber)) {
					QString releasedStation = currentStation;
					m_lastSegmentGoal.remove(agvNumber);
					// 检查是否有等待车需要重新调度
					checkAndRescheduleWaitingAgvs(releasedStation);
				}
				m_plannedSegments.erase(it);
				stopPublishTimerIfIdle();
				return;
			}

			schedulePublishAttempt();
		}

		void RouteDispatcher::ensurePublishTimerActive() {
			if (!hasPendingSegments()) return;
			if (!m_publishTimer.isActive()) {
				m_publishTimer.start();
			}
		}

		void RouteDispatcher::stopPublishTimerIfIdle() {
			if (!hasPendingSegments() && m_publishTimer.isActive()) {
				m_publishTimer.stop();
			}
		}

		bool RouteDispatcher::hasPendingSegments() const {
			for (auto it = m_plannedSegments.constBegin();
				it != m_plannedSegments.constEnd(); ++it) {
				if (!it.value().isEmpty()) {
					return true;
				}
			}
			return false;
		}

		void RouteDispatcher::schedulePublishAttempt() {
			if (!hasPendingSegments()) {
				stopPublishTimerIfIdle();
				return;
			}
			ensurePublishTimerActive();
			QMetaObject::invokeMethod(this, &RouteDispatcher::slotPublishMoveTask,
				Qt::QueuedConnection);
		}

		void RouteDispatcher::scheduleSolveAttempt() {
			// Give a short debounce window to accumulate concurrent requests
			m_solveTimer.start(50);
		}

		bool RouteDispatcher::isAgvOnThisMap(int agvNumber) const {
			auto* agv =
				m_vehicleService ? m_vehicleService->getVehicle(agvNumber) : nullptr;
			if (!agv) return false;
			return QString::fromStdString(agv->getCurrentMap()) == m_mapName;
		}

		bool RouteDispatcher::isAgvChargingOrInChargeArea(int agvNumber) const {
			auto* agv =
				m_vehicleService ? m_vehicleService->getVehicle(agvNumber) : nullptr;
			if (!agv) return false;
			if (agv->isCharging()) return true;
			// Optional: check an area named like a charge area
			int floor = m_mapService ? m_mapService->getCurrentFloor() : -1;
			auto areaMgr = m_mapService ? m_mapService->getAreaManager(floor) : nullptr;
			if (!areaMgr) return false;
			// Heuristic: if area id equals target station and occupied by this AGV
			return false;
		}

		int RouteDispatcher::selectNearestIdleAgv(const QString& goal) const {
			if (!m_mapService) {
				return -1;
			}
			auto idle = m_vehicleService->getIdleVehicles();
			double best = std::numeric_limits<double>::infinity();
			int bestNum = -1;
			for (auto* agv : idle) {
				if (QString::fromStdString(agv->getCurrentMap()) != m_mapName) continue;
				auto pos = m_mapService->getStationPosition(goal, m_floor);
				auto aPos = agv->getPosition();
				double dx = aPos.x - pos.x;
				double dy = aPos.y - pos.y;
				double d = std::sqrt(dx * dx + dy * dy);
				if (d < best) {
					best = d;
					bestNum = agv->getNumber();
				}
			}
			return bestNum;
		}


		RouteDispatcher::StartResolution RouteDispatcher::resolveStartStationWithMeta(
			const kernel::agv::AgvEntity* agv) const {
			StartResolution result;
			if (!agv) {
				if (m_logger) {
					m_logger->warning(QStringLiteral("resolveStartStation：AGV实体为空"));
				}
				return result;
			}

			const int agvNumber = agv->getNumber();
			auto logResolution = [this, agvNumber](const QString& station, const QString& reason) {
				if (!m_logger) return;
				if (station.isEmpty()) {
					m_logger->warning(QString("resolveStartStation：AGV %1 解析起点为空（原因=%2）")
						.arg(agvNumber).arg(reason));
				}
				else {
					m_logger->debug(QString("resolveStartStation：AGV %1 起点=%2（原因=%3）")
						.arg(agvNumber).arg(station).arg(reason));
				}
			};

			auto graph =
				m_mapService ? m_mapService->getPathfindingGraph(m_floor) : nullptr;

			auto isNodeValid = [&graph](const QString& node) -> bool {
				if (node.isEmpty()) return false;
				if (!graph || graph->isEmpty()) return true;
				return graph->hasNode(node);
			};

			const QString currentStation = QString::fromStdString(agv->getCurrentStation());
			const QString lastStation = QString::fromStdString(agv->getLastStation());
			const QString targetStation = QString::fromStdString(agv->getTargetStation());

			// 1) 当前站点合法直接使用
			if (isNodeValid(currentStation)) {
				result.station = currentStation;
				logResolution(result.station, QStringLiteral("当前站点"));
				return result;
			}

			// 2) 在途：current 为空且有未完成路径/目标，优先用下一目标作为起点，方便段裁剪/叠加
			QString inTransitTarget;
			const auto& unfinished = agv->getUnfinishedPath();
			if (!unfinished.empty()) {
				inTransitTarget = QString::fromStdString(unfinished.front());
			}
			else if (!targetStation.isEmpty()) {
				inTransitTarget = targetStation;
			}
			if (isNodeValid(inTransitTarget)) {
				result.station = inTransitTarget;
				result.usedTargetAsStart = true;
				logResolution(result.station, QStringLiteral("目标/未完成路径"));
				return result;
			}

			// 3) 退回到上一个经过站
			if (isNodeValid(lastStation)) {
				result.station = lastStation;
				result.usedLastStation = true;
				logResolution(result.station, QStringLiteral("上次站点"));
				return result;
			}

			// 4) 兜底：使用最近站点
			if (!m_mapService) {
				logResolution(QString(), QStringLiteral("地图服务不可用"));
				return result;
			}

			auto stationNames = m_mapService->getAllStationNames(m_floor);
			if (stationNames.isEmpty()) {
				logResolution(QString(), QStringLiteral("站点列表为空"));
				return result;
			}

			const auto agvPos = agv->getPosition();
			double bestDistance = std::numeric_limits<double>::infinity();
			QString bestStation;

			for (const auto& name : stationNames) {
				const auto pos = m_mapService->getStationPosition(name, m_floor);
				double dx = agvPos.x - pos.x;
				double dy = agvPos.y - pos.y;
				double distSq = dx * dx + dy * dy;
				if (distSq < bestDistance) {
					bestDistance = distSq;
					bestStation = name;
				}
			}

			result.station = bestStation;
			logResolution(result.station, QStringLiteral("最近站点"));
			return result;
		}

		TaskPublishHandle RouteDispatcher::stationTaskFromServer(
			const QString& station, const QString& operation,
			std::function<void()> arriveCallback) {
			// choose an idle AGV on this map
			int agvNum = selectNearestIdleAgv(station);
			if (agvNum < 0) {
				if (m_logger) m_logger->debug("StationTaskFromServer: no idle AGV");
				return { TaskFunction::FuncMove, -1, MoveFunctionStatus::MoveNoIdle };
			}
			auto handle =
				stationTaskFromServer(agvNum, station, operation, std::move(arriveCallback));
			return handle;
		}

		TaskPublishHandle RouteDispatcher::stationTaskFromServer(
			int agvNumber, const QString& station, const QString& operation,
			std::function<void()> arriveCallback) {
			auto* agv = m_vehicleService->getVehicle(agvNumber);
			if (!agv) {
				if (m_logger) {
					m_logger->error(QString("RouteDispatcher: AGV%1 not found in VehicleService")
						.arg(agvNumber));
				}
				return { TaskFunction::FuncMove, -1, MoveFunctionStatus::MoveError };
			}

			if (isAgvChargingOrInChargeArea(agvNumber)) {
				// mark controlled by dispatcher similar to third-party behavior
				agv->setControlledByDispatcher(true);
				if (m_logger) {
					m_logger->info(QString("RouteDispatcher: AGV%1 is charging, request deferred")
						.arg(agvNumber));
				}
				return { TaskFunction::FuncMove, agvNumber,
						MoveFunctionStatus::MoveCharging };
			}

			if (!isAgvOnThisMap(agvNumber)) {
				if (m_logger) {
					m_logger->warning(QString("RouteDispatcher: AGV%1 is on map '%2', expected '%3'")
						.arg(agvNumber)
						.arg(QString::fromStdString(agv->getCurrentMap()))
						.arg(m_mapName));
				}
				return { TaskFunction::FuncMove, agvNumber,
						MoveFunctionStatus::MoveNeedPassFloor };
			}

			StartResolution startInfo = resolveStartStationWithMeta(agv);
			QString start = startInfo.station;
			if (start.isEmpty()) {
				if (m_logger) {
					m_logger->error(
						QString("RouteDispatcher: unable to resolve start station for AGV%1")
						.arg(agvNumber));
				}
				return { TaskFunction::FuncMove, agvNumber, MoveFunctionStatus::MoveError };
			}

			PendingRequest req{ agvNumber, start, station, operation, std::move(arriveCallback), QDateTime::currentMSecsSinceEpoch() };
			m_pending.push_back(std::move(req));
			// 任务入口处立即缓存目标，作为唯一的任务目标来源
			m_taskGoalCache.insert(agvNumber, station);
			if (m_logger)
				m_logger->info(QString("Queued nav request AGV %1: %2 -> %3 on %4")
					.arg(agvNumber)
					.arg(start)
					.arg(station)
					.arg(m_mapName));
			if (startInfo.usedTargetAsStart || startInfo.usedLastStation) {
				m_solveFreezeAgvs.insert(agvNumber);
				if (m_logger) {
					m_logger->debug(QString("Freeze dispatch for AGV %1 until solve completes (start=%2 usedTarget=%3 usedLast=%4)")
						.arg(agvNumber)
						.arg(start)
						.arg(startInfo.usedTargetAsStart)
						.arg(startInfo.usedLastStation));
				}
			}
			if (m_vehicleService) {
				m_vehicleService->setVehicleNavTask(agvNumber, station.toStdString());
			}
			auto handle = TaskPublishHandle{ TaskFunction::FuncMove, agvNumber,
											MoveFunctionStatus::MoveOk };
			scheduleSolveAttempt();
			return handle;
		}

		/**
		 * @brief 路径求解定时器槽函数（MAPF核心入口）
		 *
		 * 工作流程：
		 * 1. 合并同AGV的多个请求（保留最早起点+最新目标）
		 * 2. 为地图上其他车辆补充占位请求（start==goal），确保求解器看到占道
		 * 3. 选择求解策略：并发多组合求解 vs 传统单线程求解
		 * 4. 将求解结果入队（enqueuePlan）
		 */
		void RouteDispatcher::slotReqPathSolveTimer() {
			const int pendingCount = static_cast<int>(m_pending.size());
			auto graph =
				m_mapService ? m_mapService->getPathfindingGraph(m_floor) : nullptr;
			if (!graph || graph->isEmpty()) {
				if (m_logger) {
					m_logger->debug(QString("路径求解跳过：图不可用 map=%1 floor=%2 pending=%3")
						.arg(m_mapName).arg(m_floor).arg(pendingCount));
				}
				return;
			}

            // ========== Lift apply filter: drop apply nodes and AGVs during elevator call ==========
            QSet<QString> liftApplyNodes;
            QSet<int> liftApplyAgvs;
			int filteredByLift = 0;
			auto isLiftApplyStage = [](lift::LiftStage stage) -> bool {
				switch (stage) {
				case lift::LiftStage::QueryStatus:
				case lift::LiftStage::QueryStatusResponse:
				case lift::LiftStage::Call:
				case lift::LiftStage::CallResponse:
				case lift::LiftStage::ArriveCallFloor:
				case lift::LiftStage::ArriveCallFloorResponse:
				case lift::LiftStage::InCloseDoor:
				case lift::LiftStage::InCloseDoorResponse:
					return true;
				default:
					return false;
				}
			};
            auto liftManager = m_mapService ? m_mapService->getLiftManager() : nullptr;
            if (liftManager) {
                const auto lifts = liftManager->getAllLifts();
                for (const auto& lift : lifts) {
                    if (!lift) continue;
                    const auto stage = lift->getStage();
                    if (!isLiftApplyStage(stage)) continue;
                    const auto& request = lift->currentRequest();
                    if (request.agvId <= 0) continue;
                    const auto& startFloorInfo = request.startFloor;
                    if (startFloorInfo.floor != m_floor) continue;
                    const QString askPoint = startFloorInfo.askPoint;
                    if (askPoint.isEmpty()) continue;
                    auto* agvEntity = m_vehicleService ? m_vehicleService->getVehicle(request.agvId) : nullptr;
                    if (!agvEntity) continue;
                    if (QString::fromStdString(agvEntity->getCurrentMap()) != m_mapName) continue;
                    liftApplyNodes.insert(askPoint);
                    liftApplyAgvs.insert(request.agvId);
                }
            }
            if (!liftApplyNodes.isEmpty()) {
				auto filteredGraph = std::make_shared<PathfindingGraph>();
				for (const auto& node : graph->getNodes()) {
					if (liftApplyNodes.contains(node.id)) {
						continue;
					}
					filteredGraph->addNode(node);
				}
				for (const auto& edge : graph->getEdges()) {
					if (liftApplyNodes.contains(edge.fromNodeId) || liftApplyNodes.contains(edge.toNodeId)) {
						continue;
					}
					filteredGraph->addEdge(edge);
				}
				graph = filteredGraph;
			}

			// ========== 第1步：合并同AGV的多个请求 ==========
			// 策略：保留最早起点，覆盖为最新目标（避免中间站点导致路径冗余）
			QHash<int, PendingRequest> mergedRequests;
			QSet<int> newRequestAgvs;  // 标记本轮新增请求的AGV
			int placeholderAdded = 0;
			auto addOrMerge = [&mergedRequests](const PendingRequest& r) {
				if (!mergedRequests.contains(r.agvNumber)) {
					mergedRequests.insert(r.agvNumber, r);
					return;
				}
				auto& existing = mergedRequests[r.agvNumber];
				// 保留最早起点，覆盖为最新目标/操作
				if (!r.goal.isEmpty()) {
					existing.goal = r.goal;
				}
				if (!r.operation.isEmpty()) {
					existing.operation = r.operation;
				}
			};

			for (const auto& r : m_pending) {
				if (liftApplyAgvs.contains(r.agvNumber) ||
					liftApplyNodes.contains(r.start) ||
					liftApplyNodes.contains(r.goal)) {
					++filteredByLift;
					continue;
				}
				addOrMerge(r);
				newRequestAgvs.insert(r.agvNumber);
			}
			// 当前批次的待处理请求已消费完毕
			m_pending.clear();
			// 注意：m_taskGoalCache 已在任务入口 stationTaskFromServer 处写入，此处无需重复

			// ========== 第2步：补充占位请求（关键：让求解器看到占道） ==========
			// 为当前地图上其他车辆（无新请求）补充占位请求（start==goal）
			// 目的：确保MAPF求解器能看到所有车辆的占道情况，避免路径冲突
			if (m_vehicleService) {
				auto vehicles = m_vehicleService->getVehiclesOnMap(m_mapName.toStdString());
				for (auto* agv : vehicles) {
					if (!agv) continue;
					if (!agv->isOnline()) {
						if (m_logger) {
							m_logger->debug(
								QString("Skip placeholder for offline AGV %1").arg(agv->getNumber()));
						}
						continue;
					}
					const int agvNum = agv->getNumber();
					if (liftApplyAgvs.contains(agvNum)) {
						continue;  // 电梯申请中的AGV不参与本轮求解
					}
					if (mergedRequests.contains(agvNum)) {
						continue;  // 已有请求，跳过
					}

					QString start = resolveStartStation(agv);
					QString goal;

					// 唯一目标来源：m_taskGoalCache（任务系统入口缓存）
					if (m_taskGoalCache.contains(agvNum)) {
						goal = m_taskGoalCache.value(agvNum);
					}
					// 无任务缓存时，作为占位请求使用起点
					if (goal.isEmpty()) {
						goal = start;  // 占位：start==goal（表示车辆静止）
					}
					if (start.isEmpty() || goal.isEmpty()) {
						continue;
					}

					PendingRequest placeholder{ agvNum, start, goal, QString(), {} };
					addOrMerge(placeholder);
					++placeholderAdded;
				}
			}

			if (mergedRequests.isEmpty()) {
				if (m_logger) {
					m_logger->debug(QString("路径求解：合并后无请求 (pending=%1 电梯过滤=%2 占位=%3)")
						.arg(pendingCount).arg(filteredByLift).arg(placeholderAdded));
				}
				return;
			}

			auto applyExistingPlanGoal = [this, &newRequestAgvs](int agvNumber, PendingRequest& request) {
				if (newRequestAgvs.contains(agvNumber)) {
					return;  // 新任务请求已有目标，不覆盖
				}
				// 唯一目标来源：m_taskGoalCache（任务系统入口缓存）
				if (m_taskGoalCache.contains(agvNumber)) {
					const auto& cachedGoal = m_taskGoalCache.value(agvNumber);
					if (!cachedGoal.isEmpty()) {
						request.goal = cachedGoal;
					}
				}
			};

			for (auto it = mergedRequests.begin(); it != mergedRequests.end(); ++it) {
				applyExistingPlanGoal(it.key(), it.value());
			}

			// ========== 第3步：构建MAPF求解请求 ==========
			std::vector<pathfinding::AgentRequest> reqs;
			std::vector<PendingRequest> activeSolveRequests;
			reqs.reserve(mergedRequests.size());
			activeSolveRequests.reserve(mergedRequests.size());
			for (auto it = mergedRequests.constBegin(); it != mergedRequests.constEnd(); ++it) {
				const auto& r = it.value();
				if (r.start.isEmpty() || r.goal.isEmpty()) {
					if (m_logger) {
						m_logger->debug(QString("Skipping MAPF request for AGV %1: start or goal empty")
							.arg(r.agvNumber));
					}
					continue;
				}
				if (liftApplyAgvs.contains(r.agvNumber) ||
					liftApplyNodes.contains(r.start) ||
					liftApplyNodes.contains(r.goal)) {
					continue;
				}

				if (r.start == r.goal) {
					// 保留 start==goal 的占位请求（用于刷新路由或重新规划）
					if (m_logger) {
						m_logger->debug(
							QString("Keeping MAPF request for AGV %1: start==goal=%2")
							.arg(r.agvNumber)
							.arg(r.start));
					}
				}

				reqs.push_back({ r.agvNumber, r.start, r.goal, r.requestedAt });
				activeSolveRequests.push_back(r);

				// 保存操作指令（到达目标后执行）
				if (!r.operation.isEmpty()) {
					m_agvOperations[r.agvNumber] = r.operation;
				}
			}

			if (reqs.empty()) {
				if (m_logger) {
					m_logger->debug(QString("路径求解：%1 个合并请求全部被过滤 (pending=%2 占位=%3 电梯过滤=%4)")
						.arg(mergedRequests.size()).arg(pendingCount).arg(placeholderAdded).arg(filteredByLift));
				}
				return;
			}

			pathfinding::MultiAgentSolver solver;
			solver.setLogger(m_logger);
			solver.setVerboseLogging(true);
			solver.buildFromGraph(graph);
			std::vector<pathfinding::AgentPlan> plans;

			bool solveSuccess = false;
			QSet<int> failedAgvs;  // 未能规划的AGV集合

			// ========== 第4步：选择求解策略 ==========
			// 条件：启用并发求解 && AGV数量>=2
			if (m_enableConcurrentSolving && reqs.size() >= 2) {
				if (m_concurrentSolveRunning) {
					if (m_logger) {
						m_logger->debug(QString("并发求解仍在进行，跳过本轮 %1 个请求").arg(reqs.size()));
					}
					return;
				}
				if (m_logger) {
					m_logger->info(QString("========== 启用并发多组合求解：%1个AGV请求 ==========").arg(reqs.size()));

					// 打印每个AGV的导航请求详情
					for (const auto& req : reqs) {
						QString requestSource = "未知来源";

						// 判断请求来源
						if (newRequestAgvs.contains(req.agvNumber)) {
							requestSource = "新任务请求";
						}
						else if (req.startId == req.goalId) {
							requestSource = "占位请求（静止）";
						}
						else {
							auto segIt = m_plannedSegments.find(req.agvNumber);
							if (segIt != m_plannedSegments.end() && !segIt.value().isEmpty()) {
								requestSource = "已有计划延续";
							}
							else {
								requestSource = "占位请求（移动中）";
							}
						}

						m_logger->info(QString("  AGV %1: %2 -> %3 on %4 [来源: %5]")
							.arg(req.agvNumber)
							.arg(req.startId)
							.arg(req.goalId)
							.arg(m_mapName)
							.arg(requestSource));
					}
					m_logger->info("========================================");
				}

				// 生成候选组合（笛卡尔积展开冲突AGV的等待替身）
				auto candidates = ConcurrentCombinationSolver::generateCandidates(reqs, m_maxConcurrentCandidates);

				// 并发求解放入后台线程，避免阻塞调度线程
				m_activeSolveRequests = activeSolveRequests;  // 记录本轮的真实请求，避免被后续新请求污染
				m_pending.clear();  // 当前批次已处理，等待新请求
				m_concurrentSolveRunning = true;
				auto future = QtConcurrent::run([candidates, graph, logger = m_logger]() {
					ConcurrentCombinationSolver concurrentSolver;
					return concurrentSolver.solveWithFallback(candidates, graph, logger, 30.0);
				});
				m_concurrentWatcher.setFuture(future);
				return;  // 后续处理在 onConcurrentSolveFinished 中完成
			}
			else {
				// ========== 传统单线程求解 ==========
				if (m_logger && !m_enableConcurrentSolving) {
					m_logger->debug("使用传统单线程MAPF求解");
				}

				if (!solver.solve(reqs, plans, 5.0)) {
					if (m_logger) m_logger->warning("Multi-agent solve failed");
					emit sigFindPathFailed();
					return;
				}
				solveSuccess = true;
				m_pending.clear();  // 当前批次已处理
			}

			if (!solveSuccess) {
				emit sigFindPathFailed();
				return;
			}

			if (m_logger) {
				for (const auto& plan : plans) {
					QStringList nodes;
					nodes.reserve(plan.route.size());
					for (const auto& node : plan.route) {
						nodes.append(node);
					}
					m_logger->debug(
						QString("MAPF plan for AGV %1: %2 (nodes=%3)")
						.arg(plan.agvNumber)
						.arg(nodes.join(" -> "))
						.arg(plan.route.size()));
				}
			}
			enqueuePlan(plans);

			if (failedAgvs.isEmpty()) {
				emit sigFindPathSuccess();
			}
			// 注意：如果有失败的AGV，不发送success信号，等待重试完成
		}

		void RouteDispatcher::onConcurrentSolveFinished() {
			m_concurrentSolveRunning = false;
			auto result = m_concurrentWatcher.result();
			m_solveFreezeAgvs.clear();

			if (!result.success) {
				if (m_logger) {
					m_logger->warning(QString("并发求解失败（不再自动重试，等待人工干预）: %1").arg(result.errorMessage));
				}
				if (m_solveFailureCallback) {
					m_solveFailureCallback(result.errorMessage);
				}
				// 不自动重试，清空当前批次
				m_activeSolveRequests.clear();
				emit sigFindPathFailed();
				return;
			}

			// 标记等待车（因目标冲突被设为 start==goal 的AGV）
			for (auto it = result.waitingAgvOriginalGoals.constBegin();
				 it != result.waitingAgvOriginalGoals.constEnd(); ++it) {
				markAgvAsWaiting(it.key(), it.value());
			}

			QSet<int> failedAgvs = result.failedAgvs;
			if (!failedAgvs.isEmpty()) {
				QStringList failedIds;
				for (int agv : failedAgvs) {
					failedIds.append(QString::number(agv));
				}
				if (m_logger) {
					m_logger->warning(QString("部分AGV未能规划: [%1]，不自动重试，等待人工调整后重新求解")
						.arg(failedIds.join(", ")));
				}
				if (m_solveFailureCallback) {
					m_solveFailureCallback(QStringLiteral("路径规划失败，受影响AGV: [%1]").arg(failedIds.join(", ")));
				}
			}

			m_activeSolveRequests.clear();

			if (m_logger) {
				for (const auto& plan : result.plans) {
					QStringList nodes;
					nodes.reserve(plan.route.size());
					for (const auto& node : plan.route) {
						nodes.append(node);
					}
					m_logger->debug(
						QString("MAPF plan for AGV %1: %2 (nodes=%3)")
						.arg(plan.agvNumber)
						.arg(nodes.join(" -> "))
						.arg(plan.route.size()));
				}
			}

			enqueuePlan(result.plans);

			if (failedAgvs.isEmpty()) {
				emit sigFindPathSuccess();
			}

			if (!m_pending.empty()) {
				scheduleSolveAttempt();
			}
		}

		void RouteDispatcher::solveAndPublishNow() {
			if (m_logger) {
				m_logger->debug(QStringLiteral("solveAndPublishNow 调用"));
			}
			slotReqPathSolveTimer();
			slotPublishMoveTask();
		}

		void RouteDispatcher::cancelPendingForAgv(int agvNumber, const QString& reason)
			{
				const std::size_t pendingSizeBefore = m_pending.size();
				// Remove pending requests waiting to be solved
				m_pending.erase(std::remove_if(m_pending.begin(), m_pending.end(),
				[agvNumber](const PendingRequest& req) {
				return req.agvNumber == agvNumber;
			}),
				m_pending.end());
			const bool pendingCleared = m_pending.size() != pendingSizeBefore;

			// Clear planned segments queue for this AGV
			auto segIt = m_plannedSegments.find(agvNumber);
			const bool hadSegments = segIt != m_plannedSegments.end();
			if (hadSegments) {
				segIt = m_plannedSegments.erase(segIt);
			}
				m_segmentCursor.remove(agvNumber);
				releaseBlockAreasForAgv(agvNumber);

				// Remove stored operations and last goal
				const bool hadOperation = m_agvOperations.remove(agvNumber) > 0;
				const bool hadLastGoal = m_lastSegmentGoal.remove(agvNumber) > 0;
				const bool hadTaskGoal = m_taskGoalCache.remove(agvNumber) > 0;

				// 清理等待车和下发待确认记录
				const bool hadWaiting = m_waitingAgvs.remove(agvNumber) > 0;
				const bool hadDispatched = m_dispatchedPending.remove(agvNumber) > 0;

			// Clear lock dependencies where this AGV is waiter or awaited
			bool lockChanged = false;
			for (auto* list : m_lockLists) {
				if (!list) continue;
				if (list->agvNum == agvNumber) {
					if (!list->tasks.isEmpty()) {
						lockChanged = true;
						list->tasks.clear();
					}
					continue;
				}
				for (auto& task : list->tasks) {
					if (task.awaited == agvNumber &&
						task.status != LockStatus::Finished) {
						lockChanged = true;
						task.status = LockStatus::Finished;
					}
				}
			}
			if (m_lockListHash.remove(agvNumber) > 0) {
				lockChanged = true;
			}

				if (m_logger && (pendingCleared || hadSegments || hadOperation ||
					hadLastGoal || hadTaskGoal || lockChanged || hadWaiting || hadDispatched)) {
					m_logger->info(QString("RouteDispatcher: cleared pending segments for AGV %1 (reason=%2)")
						.arg(agvNumber)
						.arg(reason.isEmpty() ? QStringLiteral("unspecified") : reason));
				}
			}

		/**
		 * @brief 将MAPF求解结果入队（核心：时序图处理+段队列管理）
		 *
		 * 工作流程：
		 * 1. 使用TemporalGraph分析路径时序冲突，生成等待约束
		 * 2. 将路径切分为段（segment），每段独立下发
		 * 3. 重置锁依赖，添加等待任务
		 * 4. 覆盖旧段队列（确保重算后不保留旧段）
		 *
		 * @param plans MAPF求解器返回的路径计划
		 */
		void RouteDispatcher::enqueuePlan(
			const std::vector<pathfinding::AgentPlan>& plans) {
			// ========== 第1步：时序图处理（TemporalGraph） ==========
			// 分析路径时序冲突，生成等待约束和段切分
			pathfinding::TemporalGraph tpg;
			pathfinding::TemporalGraphBuildConfig config;
			if (m_mapService) {
				auto mapService = m_mapService;
				const int dispatcherFloor = m_floor;
				// 距离提供器：计算站点间欧氏距离
				config.distanceProvider = [mapService,
					dispatcherFloor](const QString& from,
						const QString& to) -> double {
					const auto fromPos = mapService->getStationPosition(from, dispatcherFloor);
					const auto toPos = mapService->getStationPosition(to, dispatcherFloor);
					const double dx = fromPos.x - toPos.x;
					const double dy = fromPos.y - toPos.y;
					const double distance = std::sqrt(dx * dx + dy * dy);
					return distance > 0.0 ? distance : 1.0;
				};
				config.safetyDistance = 0.2;  // 安全距离（米）
				config.maxSpeed = 1.0;        // 最大速度（米/秒）
			}
			auto result = tpg.build(plans, config);
			m_specialStationWhiteList = result.specialStationWhiteList;

			if (m_logger) {
				m_logger->info("===== TGP (Temporal Graph Processing) Result =====");
				m_logger->debug(
					QString("TemporalGraph result: segments for %1 AGVs, waitConstraints=%2")
					.arg(result.segmentsByAgv.size())
					.arg(result.waitConstraints.size()));

				// Print detailed segments for each AGV
				for (auto it = result.segmentsByAgv.constBegin();
					it != result.segmentsByAgv.constEnd(); ++it) {
					int agvNum = it.key();
					const auto& segList = it.value();
					m_logger->info(QString("AGV %1 has %2 segment(s):")
						.arg(agvNum)
						.arg(segList.size()));
					for (int idx = 0; idx < segList.size(); ++idx) {
						const auto& seg = segList.at(idx);
						QStringList nodes;
						nodes.reserve(seg.size());
						for (const auto& s : seg) {
							nodes.append(s);
						}
						m_logger->info(QString("  Segment %1: %2")
							.arg(idx + 1)
							.arg(nodes.join(" -> ")));
					}
				}

				// Print wait constraints
				if (!result.waitConstraints.isEmpty()) {
					m_logger->info(QString("Wait Constraints (%1):").arg(result.waitConstraints.size()));
				}
				auto segmentLabelFor = [&](int agv, int stationIndex) -> QString {
					auto segIt = result.segmentsByAgv.constFind(agv);
					if (segIt == result.segmentsByAgv.constEnd()) {
						return {};
					}
					const auto& segList = segIt.value();
					if (stationIndex < 0 || stationIndex >= segList.size()) {
						return {};
					}
					const auto& seg = segList.at(stationIndex);
					QStringList nodes;
					nodes.reserve(seg.size());
					for (const auto& s : seg) {
						nodes.append(s);
					}
					return nodes.join(" -> ");
				};
				for (const auto& wait : result.waitConstraints) {
					const QString segmentDesc =
						segmentLabelFor(wait.waiterAgv, wait.waiterStationIndex);
					const QString dispatchSeg =
						segmentDesc.isEmpty() ? QStringLiteral("待下发段缺失") : segmentDesc;
					m_logger->info(QString("  AGV %1 等待 AGV %2 pass %3->%4，下发 %5")
						.arg(wait.waiterAgv)
						.arg(wait.awaitedAgv)
						.arg(wait.unlockFrom)
						.arg(wait.unlockTo)
						.arg(dispatchSeg));
				}
				m_logger->info("===== End of TGP Result =====");
			}

			// ========== 第2步：重置锁依赖 ==========
			// 收集所有涉及重新规划的AGV（包括等待约束中的waiter和awaited）
			QSet<int> replanAgvs;
			for (const auto& plan : plans) {
				replanAgvs.insert(plan.agvNumber);
			}
			for (const auto& wait : result.waitConstraints) {
				if (wait.waiterAgv > 0) {
					replanAgvs.insert(wait.waiterAgv);
				}
				if (wait.awaitedAgv > 0) {
					replanAgvs.insert(wait.awaitedAgv);
				}
			}
			resetLocksForAgvs(replanAgvs);

			// ========== 第3步：添加等待任务 ==========
			// 根据时序图生成的等待约束，建立AGV间的依赖关系
			for (const auto& wc : result.waitConstraints) {
				addWaitTask(0, wc.awaitedAgv, wc.unlockFrom, wc.unlockTo, wc.waiterAgv,
					wc.waiterStationIndex);
			}

			// ========== 第4步：覆盖段队列（关键：清空旧段） ==========
			// 将时序图切分的段追加到各AGV的队列中
			// 注意：覆盖旧队列，确保重算后不保留旧段
			for (auto it = result.segmentsByAgv.constBegin();
				it != result.segmentsByAgv.constEnd(); ++it) {
				const int agvNum = it.key();
				auto& queue = m_plannedSegments[agvNum];
				queue.clear();  // 清空旧段队列
				const auto& segList = it.value();
				for (int idx = 0; idx < segList.size(); ++idx) {
					queue.append(segList.at(idx));
				}
				m_segmentCursor.insert(agvNum, 0);  // 重置段游标
				m_lastSegmentGoal.remove(agvNum);   // 清除上次目标
			}

			// 触发段下发尝试
			schedulePublishAttempt();
		}

		void RouteDispatcher::slotPublishMoveTask() {
			updateLocks();
			if (m_logger) {
				int agvCount = m_plannedSegments.size();
				int segCount = 0;
				for (auto it = m_plannedSegments.constBegin(); it != m_plannedSegments.constEnd(); ++it) {
					segCount += it.value().size();
				}
				m_logger->debug(QString("段下发轮询：计划中的AGV=%1 段队列总数=%2").arg(agvCount).arg(segCount));
			}
			// Iterate AGVs and publish first pending segment once previous segment finished
			for (auto it = m_plannedSegments.begin(); it != m_plannedSegments.end();
				++it) {
				int agv = it.key();
				if (m_solveFreezeAgvs.contains(agv)) {
					if (m_logger) {
						m_logger->debug(
							QString("段下发冻结：AGV %1 等待求解结果").arg(agv));
					}
					continue;
				}
				auto* agvEntity =
					m_vehicleService ? m_vehicleService->getVehicle(agv) : nullptr;
				if (!agvEntity) {
					if (m_logger) {
						m_logger->warning(
							QString("Segment dispatch skipped for AGV %1: vehicle not found")
							.arg(agv));
					}
					continue;
				}

				auto& queue = it.value();
				if (m_logger) {
					m_logger->debug(QString("段下发检查：AGV %1 队列大小=%2 上次目标=%3")
						.arg(agv)
						.arg(queue.size())
						.arg(m_lastSegmentGoal.contains(agv) ? m_lastSegmentGoal.value(agv) : QStringLiteral("<none>")));
				}
				if (queue.isEmpty()) {
					m_segmentCursor.remove(agv);
					QString station =
						QString::fromStdString(agvEntity->getCurrentStation());
					if (station.isEmpty()) {
						station = QString::fromStdString(agvEntity->getLastStation());
					}
					// 清理上一段目标
					if (m_lastSegmentGoal.contains(agv) && !station.isEmpty() &&
						station == m_lastSegmentGoal.value(agv)) {
						m_lastSegmentGoal.remove(agv);
					}
					// 只有当车辆已到达任务最终目标时才清理任务目标缓存
					if (m_taskGoalCache.contains(agv) && !station.isEmpty() &&
						station == m_taskGoalCache.value(agv)) {
						m_taskGoalCache.remove(agv);
						if (m_logger) {
							m_logger->info(QString("AGV %1 已到达任务目标 %2，清除目标缓存")
								.arg(agv).arg(station));
						}
					}
					// No pending segment: release any block-area reservations for this AGV
					releaseBlockAreasForAgv(agv);
					if (m_logger) {
						m_logger->debug(
							QString("段下发跳过：AGV %1 队列为空，已清理游标和占用").arg(agv));
					}
					continue;
				}

				QString currentStation =
					QString::fromStdString(agvEntity->getCurrentStation());
				if (currentStation.isEmpty()) {
					currentStation = QString::fromStdString(agvEntity->getLastStation());
				}
				if (currentStation.isEmpty()) {
					if (m_logger) {
						m_logger->debug(QString("段下发跳过：AGV %1 当前站点未知，队列大小=%2")
							.arg(agv)
							.arg(queue.size()));
					}
				}
				const auto navStatus = agvEntity->getNavStatus();
					auto navStatusToString = [](kernel::agv::NavStatus status) -> QString {
						switch (status) {
						case kernel::agv::NavStatus::None: return QStringLiteral("None");
						case kernel::agv::NavStatus::Running: return QStringLiteral("Running");
					case kernel::agv::NavStatus::Suspended: return QStringLiteral("Suspended");
					case kernel::agv::NavStatus::Canceled: return QStringLiteral("Canceled");
					case kernel::agv::NavStatus::Completed: return QStringLiteral("Completed");
					case kernel::agv::NavStatus::Failed: return QStringLiteral("Failed");
					default: return QStringLiteral("Unknown");
					}
				};
				if (m_lastSegmentGoal.contains(agv)) {
					const auto& expectedGoal = m_lastSegmentGoal.value(agv);
					const bool reachedExpected = !expectedGoal.isEmpty() && currentStation == expectedGoal;
					const bool navActive = navStatus == kernel::agv::NavStatus::Running;
					if (!expectedGoal.isEmpty() && !reachedExpected) {
						if (navActive) {
							// 等待车辆真实到达上一段终点，避免提前计算后续段造成死锁误判
							if (m_logger) {
								QStringList nodes;
								if (!queue.isEmpty()) {
									const auto& pendingSeg = queue.first();
									nodes.reserve(pendingSeg.size());
									for (const auto& station : pendingSeg) {
										nodes.append(station);
									}
								}
								m_logger->debug(
									QString("段下发等待：AGV %1 current=%2 targetLast=%3 nav=%4 queueSize=%5 pending=%6")
									.arg(agv)
									.arg(currentStation.isEmpty() ? QStringLiteral("<unknown>")
										: currentStation)
									.arg(expectedGoal)
									.arg(navStatusToString(navStatus))
									.arg(queue.size())
									.arg(nodes.isEmpty() ? QStringLiteral("<none>") : nodes.join(" -> ")));
							}
							continue;
						}
					}
				}

				int nextSegmentIdx = nextSegmentIndex(agv);
				if (m_logger) {
					m_logger->debug(QString("段下发尝试：AGV %1 nextIdx=%2 queueSize=%3 current=%4 nav=%5")
						.arg(agv)
						.arg(nextSegmentIdx)
						.arg(queue.size())
						.arg(currentStation.isEmpty() ? QStringLiteral("<unknown>") : currentStation)
						.arg(navStatusToString(navStatus)));
				}
				QSet<int> blockingAwaited;
				if (hasBlockingLock(agv, nextSegmentIdx, &blockingAwaited)) {
					if (m_logger) {
						QStringList awaitedList;
						for (int a : blockingAwaited) {
							awaitedList.append(QString::number(a));
						}
						m_logger->debug(
							QString("段下发阻塞：AGV %1 存在锁依赖，等待=%2")
							.arg(agv)
							.arg(awaitedList.isEmpty() ? QStringLiteral("<unknown>") : awaitedList.join(",")));
					}
					if (!blockingAwaited.isEmpty()) {
						QSet<int> involved;
						if (detectMutualLock(agv, blockingAwaited, involved)) {
							resolveDeadlock(involved);
							return;
							}
						}
						continue;
					}
					const auto seg = queue.first();
					if (seg.size() < 2) {
						queue.removeFirst();
						advanceSegmentCursor(agv);
						if (queue.isEmpty()) {
							m_segmentCursor.remove(agv);
							releaseBlockAreasForAgv(agv);
						}
						continue;
					}
					const QSet<QString> segBlockAreas = collectBlockAreasForPath(seg);
					auto areaMgr = m_mapService ? m_mapService->getAreaManager(m_floor) : nullptr;
					if (areaMgr && !segBlockAreas.isEmpty()) {
						QString conflictArea;
						int conflictAgv = -1;
						for (const auto& areaId : segBlockAreas) {
							auto area = areaMgr->getArea(areaId);
							if (area) {
								const auto occupants = area->getOccupyingAgvIds();
								for (int occ : occupants) {
									if (occ != agv) {
										conflictArea = areaId;
										conflictAgv = occ;
										break;
									}
								}
							}
							if (!conflictArea.isEmpty()) {
								break;
							}
						}
						// 如果只有本车占用/预约或未知占用，则不阻塞下发
						if (!conflictArea.isEmpty() && conflictAgv > 0 && conflictAgv != agv) {
							bool alreadyWaiting = false;
							if (m_lockListHash.contains(agv)) {
								const auto* list = m_lockListHash.value(agv);
								for (const auto& task : list->tasks) {
									if (task.unlockStationFrom == seg.first() &&
										task.unlockStation == seg.last() &&
										task.status == LockStatus::Locking) {
										alreadyWaiting = true;
										break;
									}
								}
							}
							if (!alreadyWaiting) {
								addWaitTask(0, conflictAgv, seg.first(), seg.last(), agv, nextSegmentIdx,
									LockStatus::Locking);
								if (m_logger) {
									m_logger->debug(QString("AGV %1 waits BlockArea %2 (occupied by %3)")
										.arg(agv)
										.arg(conflictArea)
										.arg(conflictAgv > 0 ? QString::number(conflictAgv) : QStringLiteral("unknown")));
								}
								// 如果双方互为阻塞，立即触发解锁/重算，避免无限等待
								if (conflictAgv > 0) {
									QSet<int> involved;
									QSet<int> blockingAwaited;
									blockingAwaited.insert(conflictAgv);
									if (detectMutualLock(agv, blockingAwaited, involved)) {
										resolveDeadlock(involved);
										return;
									}
								}
							}
							continue;
						}
					}

				const bool isLastSegment = (queue.size() == 1);
				std::string operation;
				if (isLastSegment && m_agvOperations.contains(agv)) {
					operation = m_agvOperations[agv].toStdString();
				}

				const QString targetStation =
					QString::fromStdString(agvEntity->getTargetStation());
				const bool targetInSegment = !targetStation.isEmpty() && seg.contains(targetStation);

				auto toStdStations = [](const QStringList& path) {
					std::vector<std::string> out;
					out.reserve(path.size());
					for (const auto& s : path) {
						out.emplace_back(s.toStdString());
						}
						return out;
					};

					// 导航运行中且目标在当前段内
					if (navStatus == kernel::agv::NavStatus::Running && targetInSegment) {
						auto dispatchPath = [&](const QStringList& path) -> bool {
							if (path.size() < 2) {
								return false;
							}
						auto vec = toStdStations(path);
						auto res = m_vehicleService->moveVehicleToStations(agv, vec, operation);
						if (!res.isSuccess() && m_logger) {
							m_logger->warning(QString("Dispatch in-running segment failed for AGV %1 (%2 -> %3)")
								.arg(agv)
								.arg(path.isEmpty() ? QStringLiteral("<empty>") : path.first())
								.arg(path.isEmpty() ? QStringLiteral("<empty>") : path.last()));
						} else if (res.isSuccess() && m_logger) {
							m_logger->info(QString("段下发成功（导航运行中） AGV %1: %2 -> %3")
								.arg(agv)
								.arg(path.isEmpty() ? QStringLiteral("<empty>") : path.first())
								.arg(path.isEmpty() ? QStringLiteral("<empty>") : path.last()));
						}
						return res.isSuccess();
					};

						if (currentStation.isEmpty()) {
							// 当前站点缺失：若当前目标等于段首，无需截断，直接下发整段，带退避
							if (!targetStation.isEmpty() && targetStation == seg.first()) {
								auto now = QDateTime::currentDateTime();
								auto retryIt = m_emptyStationRetryAt.constFind(agv);
								if (retryIt != m_emptyStationRetryAt.constEnd() && now < retryIt.value()) {
									if (m_logger) {
										m_logger->debug(QString("Dispatch (empty current, target==start) backoff for AGV %1: wait %2 ms")
											.arg(agv)
											.arg(now.msecsTo(retryIt.value())));
									}
								}
								else {
									if (m_logger) {
										m_logger->info(QString("Dispatch running segment (empty current, target==start) for AGV %1: %2 -> %3")
											.arg(agv)
											.arg(seg.first())
											.arg(seg.last()));
									}
									dispatchPath(seg);
									m_emptyStationRetryAt[agv] = now.addMSecs(EmptyStationBackoffMs);
								}
								continue;
							}

							QStringList truncated;
							for (const auto& s : seg) {
								truncated.append(s);
								if (s == targetStation) break;
							}
							if (truncated.size() < 2) {
								truncated = seg; // fallback to full segment
							}
							if (m_logger) {
								m_logger->info(QString("Dispatch running segment (empty current) for AGV %1: %2 -> %3")
									.arg(agv)
									.arg(truncated.first())
									.arg(truncated.last()));
							}
							dispatchPath(truncated);
							m_emptyStationRetryAt[agv] = QDateTime::currentDateTime().addMSecs(EmptyStationBackoffMs);
							continue;
						}

						if (seg.contains(currentStation)) {
							QStringList truncated;
							bool collect = false;
							for (const auto& s : seg) {
								if (!collect && s == currentStation) {
									collect = true;
								}
								if (collect) {
									truncated.append(s);
								}
								if (s == targetStation) {
									break;
								}
							}
							if (truncated.size() >= 2) {
								if (m_logger) {
									m_logger->info(QString("Dispatch running segment for AGV %1: %2 -> %3")
										.arg(agv)
										.arg(truncated.first())
										.arg(truncated.last()));
								}
								if (dispatchPath(truncated)) {
									continue;
								}
							}
						}
						else {
							const auto now = QDateTime::currentDateTime();
							auto retryIt = m_emptyStationRetryAt.constFind(agv);
							if (retryIt != m_emptyStationRetryAt.constEnd() && now < retryIt.value()) {
								if (m_logger) {
									m_logger->debug(QString("Dispatch fallback (running) backoff for AGV %1: wait %2 ms")
										.arg(agv)
										.arg(now.msecsTo(retryIt.value())));
								}
								continue;
							}
							// 如果车辆已在导航且目标与当前段终点一致，则不重复兜底
							if (navStatus == kernel::agv::NavStatus::Running &&
								!targetStation.isEmpty() &&
								targetStation == seg.first()) {
								if (m_logger) {
									m_logger->info(QString("Skip fallback (running) for AGV %1: aligning to %2 already navigating")
										.arg(agv)
										.arg(targetStation));
								}
								m_emptyStationRetryAt[agv] = now.addMSecs(EmptyStationBackoffMs);
								continue;
							}
							auto fbRes = m_vehicleService->moveVehicleToStation(
								agv, "SELF_POSITION", seg.first().toStdString());
							if (fbRes.isSuccess()) {
								if (m_logger) {
									m_logger->info(QString("兜底下发（运行态） AGV %1: SELF_POSITION -> %2, current=%3 target=%4 nav=%5 segStart=%6 segEnd=%7")
										.arg(agv)
										.arg(seg.first())
										.arg(currentStation.isEmpty() ? QStringLiteral("<empty>") : currentStation)
										.arg(targetStation.isEmpty() ? QStringLiteral("<empty>") : targetStation)
										.arg(navStatusToString(navStatus))
										.arg(seg.first())
										.arg(seg.isEmpty() ? QStringLiteral("<empty>") : seg.last()));
								}
								m_emptyStationRetryAt[agv] = now.addMSecs(EmptyStationBackoffMs);
							}
							else if (m_logger) {
								m_logger->warning(QString("兜底下发失败（运行态） AGV %1: SELF_POSITION -> %2, current=%3 target=%4 nav=%5")
									.arg(agv)
									.arg(seg.isEmpty() ? QStringLiteral("<empty>") : seg.first())
									.arg(currentStation.isEmpty() ? QStringLiteral("<empty>") : currentStation)
									.arg(targetStation.isEmpty() ? QStringLiteral("<empty>") : targetStation)
									.arg(navStatusToString(navStatus)));
							}
							m_emptyStationRetryAt[agv] = QDateTime::currentDateTime().addMSecs(EmptyStationBackoffMs);
							continue;
						}
					}

					// 非运行状态或目标不在段内
					const auto now = QDateTime::currentDateTime();
					if (currentStation.isEmpty()) {
						auto retryIt = m_emptyStationRetryAt.constFind(agv);
						if (retryIt != m_emptyStationRetryAt.constEnd() && now < retryIt.value()) {
							if (m_logger) {
								m_logger->debug(QString("Segment dispatch backoff for AGV %1: currentStation empty, wait %2 ms")
									.arg(agv)
									.arg(now.msecsTo(retryIt.value())));
							}
							continue;
						}
						auto fbRes = m_vehicleService->moveVehicleToStation(
							agv, "SELF_POSITION", seg.first().toStdString());
						if (fbRes.isSuccess()) {
							if (m_logger) {
								m_logger->info(QString("兜底下发（空站点） AGV %1: SELF_POSITION -> %2，对齐段首，current=<empty> target=%3 nav=%4 segStart=%5 segEnd=%6")
									.arg(agv)
									.arg(seg.first())
									.arg(targetStation.isEmpty() ? QStringLiteral("<empty>") : targetStation)
									.arg(navStatusToString(navStatus))
									.arg(seg.first())
									.arg(seg.isEmpty() ? QStringLiteral("<empty>") : seg.last()));
							}
						}
						else if (m_logger) {
							m_logger->warning(QString("兜底下发失败（空站点） AGV %1: SELF_POSITION -> %2，对齐段首，current=<empty> target=%3 nav=%4")
								.arg(agv)
								.arg(seg.isEmpty() ? QStringLiteral("<empty>") : seg.first())
								.arg(targetStation.isEmpty() ? QStringLiteral("<empty>") : targetStation)
								.arg(navStatusToString(navStatus)));
						}
						m_emptyStationRetryAt[agv] = now.addMSecs(EmptyStationBackoffMs);
						continue;
					}
					m_emptyStationRetryAt.remove(agv);

					// Third-party area check: if occupied by others, wait
					auto thirdMgr =
						m_mapService ? m_mapService->getThirdAreaManager(m_floor) : nullptr;
					auto autoDoorMgr =
						m_mapService ? m_mapService->getAutoDoorManager(m_floor) : nullptr;
					if (thirdMgr && m_mapService) {
						bool mustWait = false;
						QSet<QString> checkedAreas;
						for (const auto& s : seg) {
							const auto areaId = m_mapService->getAreaIdForStation(m_floor, s);
							if (areaId.isEmpty() || checkedAreas.contains(areaId)) {
								continue;
							}
							checkedAreas.insert(areaId);
							auto area = thirdMgr->getArea(areaId);
							if (!area || area->getFloor() != m_floor) {
								continue;
							}
							if (!area->tryAgvAcquire(agv)) {
								mustWait = true;
								break;
							}
						}
						if (mustWait) {
							bool alreadyWaiting = false;
							if (m_lockListHash.contains(agv)) {
								const auto* list = m_lockListHash.value(agv);
								for (const auto& task : list->tasks) {
									if (task.unlockStationFrom == seg.first() &&
										task.unlockStation == seg.last() &&
										task.status == LockStatus::Locking) {
										alreadyWaiting = true;
										break;
									}
								}
							}
							if (!alreadyWaiting) {
								addWaitTask(0, -1, seg.first(), seg.last(), agv, nextSegmentIdx,
									LockStatus::Locking);
								if (m_logger) {
									m_logger->info(
										QString("AGV %1 waits for third-area release on %2 -> %3")
										.arg(agv)
										.arg(seg.first())
										.arg(seg.last()));
								}
							}
							continue;
						}
					}
					std::vector<std::string> stations;
					stations.reserve(seg.size());
					for (const auto& s : seg) stations.emplace_back(s.toStdString());

					const int remainingSegments = queue.size() - 1;
					if (m_logger) {
						QStringList nodes;
						nodes.reserve(seg.size());
						for (const auto& s : seg) nodes.append(s);
						m_logger->debug(
							QString("Dispatching segment for AGV %1: %2 (remaining segments=%3)")
							.arg(agv)
							.arg(nodes.join(" -> "))
							.arg(remainingSegments));
					}

					if (autoDoorMgr && !seg.isEmpty()) {
						auto triggerDoorForPath = [&](const QStringList& path) {
							for (int idx = 0; idx < path.size() - 1; ++idx) {
								const QString& from = path.at(idx);
								const QString& to = path.at(idx + 1);
								auto door = autoDoorMgr->findDoorBySegment(from, to);
								if (door) {
									autoDoorMgr->openDoor(door->getDoorId());
									if (m_logger) {
										m_logger->debug(QString("自动门触发: 楼层%1, 门ID=%2, 路径=%3->%4")
											.arg(m_floor)
											.arg(door->getDoorId())
											.arg(from)
											.arg(to));
									}
								}
							}
						};

						triggerDoorForPath(seg);
						if (queue.size() > 1) {
							const auto& nextSeg = queue.at(1);
							triggerDoorForPath(nextSeg);
							if (!seg.isEmpty() && !nextSeg.isEmpty()) {
								auto door = autoDoorMgr->findDoorBySegment(seg.last(), nextSeg.first());
								if (door) {
									autoDoorMgr->openDoor(door->getDoorId());
									if (m_logger) {
										m_logger->debug(QString("自动门预触发: 楼层%1, 门ID=%2, 路径=%3->%4")
											.arg(m_floor)
											.arg(door->getDoorId())
											.arg(seg.last())
											.arg(nextSeg.first()));
									}
								}
							}
						}
					}

					const bool startMismatch =
						currentStation.isEmpty() || seg.first() != currentStation;
					kernel::Result<void> res;
					if (startMismatch) {
						// 起点与车端不符：若车已在段内则截断下发，否则以当前位置重算
						if (currentStation.isEmpty()) {
							if (m_logger) {
								m_logger->warning(QString("Dispatch skipped: AGV %1 currentStation empty, expected=%2 idx=%3 queueSize=%4")
									.arg(agv)
									.arg(seg.first())
									.arg(nextSegmentIdx)
									.arg(queue.size()));
							}
							// 等待下一次状态上报
							continue;
						}
						const bool currentInSegment = seg.contains(currentStation);
						if (currentInSegment) {
							if (m_logger) {
								m_logger->info(QString("Start mismatch detected for AGV %1: expected=%2, current=%3, idx=%4 -> truncate segment")
									.arg(agv)
									.arg(seg.first())
									.arg(currentStation)
									.arg(nextSegmentIdx));
							}
							// 车在段内，截去已走过的部分
							QStringList truncated;
							bool collect = false;
							for (const auto& s : seg) {
								if (!collect && s == currentStation) {
									collect = true;
								}
								if (collect) truncated.append(s);
							}
							if (truncated.size() < 2) {
								// 已到段尾，视为该段完成
								queue.removeFirst();
								advanceSegmentCursor(agv);
								if (queue.isEmpty()) {
									m_segmentCursor.remove(agv);
								}
								continue;
							}
							stations.clear();
							stations.reserve(truncated.size());
							for (const auto& s : truncated) stations.emplace_back(s.toStdString());
							if (m_logger) {
								m_logger->info(
									QString("Adjust segment for AGV %1: current=%2, truncate and dispatch %3 -> %4")
									.arg(agv)
									.arg(currentStation)
									.arg(truncated.first())
									.arg(truncated.last()));
							}
						}
						else {
							// 不在段内，根据导航状态决定处理方式

							// 情况1：已经在目标点但不在段内，视为该段完成
							if (targetInSegment && currentStation == targetStation) {
								if (m_logger) {
									m_logger->info(QString("Start mismatch skip fallback for AGV %1: current==target==%2, finish segment")
										.arg(agv)
										.arg(currentStation));
								}
								queue.removeFirst();
								advanceSegmentCursor(agv);
								if (queue.isEmpty()) {
									m_segmentCursor.remove(agv);
									releaseBlockAreasForAgv(agv);
								}
								continue;
							}

							// 情况2：导航正在运行中，等待导航完成
							if (navStatus == kernel::agv::NavStatus::Running) {
								if (m_logger) {
									m_logger->debug(QString("Start mismatch wait for AGV %1: nav running, current=%2 target=%3 segStart=%4")
										.arg(agv)
										.arg(currentStation)
										.arg(targetStation)
										.arg(seg.first()));
								}
								continue;
							}

							// 情况3：导航失败，需要兜底下发
							if (navStatus == kernel::agv::NavStatus::Failed) {
								// 退避检查
								auto retryIt = m_emptyStationRetryAt.constFind(agv);
								if (retryIt != m_emptyStationRetryAt.constEnd() && QDateTime::currentDateTime() < retryIt.value()) {
									if (m_logger) {
										m_logger->debug(QString("兜底下发退避（导航失败） AGV %1: wait %2 ms")
											.arg(agv)
											.arg(QDateTime::currentDateTime().msecsTo(retryIt.value())));
									}
									continue;
								}
								// 兜底：使用车端当前位置对齐到段起点
								auto fbRes = m_vehicleService->moveVehicleToStation(
									agv, "SELF_POSITION", seg.first().toStdString());
								if (fbRes.isSuccess()) {
									m_emptyStationRetryAt[agv] = QDateTime::currentDateTime().addMSecs(EmptyStationBackoffMs);
									if (m_logger) {
										m_logger->info(QString("兜底下发（导航失败） AGV %1: SELF_POSITION -> %2, current=%3 segEnd=%4")
											.arg(agv)
											.arg(seg.first())
											.arg(currentStation)
											.arg(seg.last()));
									}
									continue;
								}
							}

							continue;
						}
					}
					if (stations.size() < 2) {
						if (m_logger) {
							m_logger->warning(
								QString("Dispatch skipped: AGV %1 segment station count <2 (count=%2) %3")
								.arg(agv)
								.arg(stations.size())
								.arg(seg.isEmpty() ? QStringLiteral("<empty>") : seg.join("->")));
						}
						queue.removeFirst();
						advanceSegmentCursor(agv);
						if (queue.isEmpty()) {
							m_segmentCursor.remove(agv);
						}
						continue;
					}
				res = m_vehicleService->moveVehicleToStations(agv, stations, operation);
				const bool sameRouteSkipped = (!res.isSuccess() &&
					res.getErrorCode() == kernel::ErrorCode::InvalidState);
				if (!res.isSuccess() && m_logger) {
					if (sameRouteSkipped) {
							// Vehicle reported identical route; keep cursor/queue intact to retry when it finishes.
							m_logger->info(QString("Dispatch segment skipped for AGV %1: identical route already running")
								.arg(agv));
						}
						else {
							m_logger->warning(QString("Dispatch segment failed for AGV %1").arg(agv));
					}
				}
				if (res.isSuccess()) {
					if (m_logger) {
						m_logger->info(QString("段下发成功 AGV %1: %2 -> %3 (len=%4)")
							.arg(agv)
							.arg(seg.isEmpty() ? QStringLiteral("<empty>") : seg.first())
							.arg(seg.isEmpty() ? QStringLiteral("<empty>") : seg.last())
							.arg(seg.size()));
					}
					queue.removeFirst();
					advanceSegmentCursor(agv);
					if (queue.isEmpty()) {
						m_segmentCursor.remove(agv);
					}
						m_lastSegmentGoal.insert(agv, seg.last());

						// 记录待确认的下发（导航状态监控）
						DispatchedSegmentInfo dispatchInfo;
						dispatchInfo.agvNumber = agv;
						dispatchInfo.segment = seg;
						dispatchInfo.dispatchedAt = QDateTime::currentDateTime();
						dispatchInfo.retryCount = 0;
						m_dispatchedPending.insert(agv, dispatchInfo);
						// 确保状态检查定时器运行
						if (!m_stateCheckTimer.isActive()) {
							m_stateCheckTimer.start();
						}

						if (m_dispatchCallback) {
							m_dispatchCallback(agv, seg);
						}
						// After issuing the segment, finalize any unlocked locks for this waiter
						finalizeLocksForWaiter(agv);
						// If this was the last segment, clear the operation
						if (isLastSegment) {
							if (m_logger) {
								m_logger->debug(
									QString("All segments dispatched for AGV %1").arg(agv));
							}
							m_agvOperations.remove(agv);
						}
					}
				}
			}

		void RouteDispatcher::addWaitTask(int routeIndex, int awaited,
			const QString& unlockFrom,
			const QString& unlockTo, int waiter,
			int stationIndex, LockStatus initialStatus) {
			// Skeleton implementation: track wait dependency between AGVs
			LockList* list = nullptr;
			if (m_lockListHash.contains(waiter)) {
				list = m_lockListHash.value(waiter);
			}
			else {
				list = new LockList{ waiter, {} };
				m_lockLists.append(list);
				m_lockListHash.insert(waiter, list);
			}
			list->tasks.append(
				LockTask{ awaited, unlockFrom, unlockTo, waiter, initialStatus, stationIndex });
			if (m_logger)
				m_logger->debug(QString("AGV %1 waits for AGV %2 to pass %3 -> %4")
					.arg(waiter)
					.arg(awaited)
					.arg(unlockFrom)
					.arg(unlockTo));
			if (m_logger) {
				m_logger->info(QString("AddWaitTask waiter=%1 awaited=%2 unlock=%3->%4 stationIdx=%5 status=%6")
					.arg(waiter)
					.arg(awaited)
					.arg(unlockFrom)
					.arg(unlockTo)
					.arg(stationIndex)
					.arg(lockStatusToString(initialStatus)));
			}
		}

		void RouteDispatcher::onAreaExited(int agvId, const QString& areaId) {
			// Resume tasks waiting on this AGV/area; mark locks as unlocked
			bool changed = false;
			bool blockReleased = false;
			if (m_mapService && m_mapService->isBlockArea(m_floor, areaId)) {
				if (m_blockAreaReservations.value(areaId) == agvId) {
					m_blockAreaReservations.remove(areaId);
					blockReleased = true;
				}
				auto it = m_agvReservedBlockAreas.find(agvId);
				if (it != m_agvReservedBlockAreas.end()) {
					it->remove(areaId);
					if (it->isEmpty()) {
						m_agvReservedBlockAreas.erase(it);
					}
					blockReleased = true;
				}
				if (blockReleased && m_logger) {
					m_logger->info(QString("BlockArea release detected from area exit: area=%1 agv=%2")
						.arg(areaId)
						.arg(agvId));
				}
			}
			for (auto* list : m_lockLists) {
				for (auto& t : list->tasks) {
					if (t.awaited == agvId && t.status == LockStatus::Locking) {
						// Only unlock when the exited area matches the lock's segment endpoints
						if (!areaId.isEmpty() &&
							areaId != t.unlockStationFrom &&
							areaId != t.unlockStation) {
							continue;
						}
						t.status = LockStatus::UnLock;
						clearLockBlockLogKey(t, t.waiter);
						changed = true;
						if (m_logger) {
							m_logger->debug(QString("Lock unlocked by area exit: awaited=%1 waiter=%2 unlock=%3->%4")
								.arg(t.awaited)
								.arg(t.waiter)
								.arg(t.unlockStationFrom)
								.arg(t.unlockStation));
						}
					}
				}
			}
			// 已有路径仍有效，只需继续尝试下发，无需重新规划
			if (changed || blockReleased) {
				schedulePublishAttempt();
			}
		}

		void RouteDispatcher::updateLocks() {
			// When awaited AGV reaches unlockStation, mark corresponding locks as
			// UnLock/Finished
			for (auto* list : m_lockLists) {
				for (auto& t : list->tasks) {
					if (t.status == LockStatus::Finished) continue;
					auto* awaitedAgv = m_vehicleService->getVehicle(t.awaited);
					if (!awaitedAgv) continue;
					QString curr = QString::fromStdString(awaitedAgv->getCurrentStation());
					if (!curr.isEmpty() && curr == t.unlockStation) {
						t.status = LockStatus::UnLock;
						clearLockBlockLogKey(t, t.waiter);
						if (m_logger) {
							m_logger->info(QString("Lock unlocked by station reach: awaited=%1 waiter=%2 unlock=%3->%4")
								.arg(t.awaited)
								.arg(t.waiter)
								.arg(t.unlockStationFrom)
								.arg(t.unlockStation));
						}
					}
				}
			}
		}

		bool RouteDispatcher::hasBlockingLock(int waiterAgv, int upcomingSegmentIndex,
			QSet<int>* blockingAwaited) {
			auto it = m_lockListHash.find(waiterAgv);
			if (it == m_lockListHash.end()) return false;
			auto* list = it.value();
			bool blocked = false;
			for (auto& task : list->tasks) {
				if (task.status == LockStatus::Finished ||
					task.status == LockStatus::UnLock) {
					continue;
				}
				if (task.status == LockStatus::Pending) {
					if (upcomingSegmentIndex >= task.stationsIndex) {
						if (m_logger) {
							m_logger->debug(QString("Lock pending->locking waiter=%1 awaited=%2 idx=%3 upcomingIdx=%4 unlock=%5->%6")
								.arg(waiterAgv)
								.arg(task.awaited)
								.arg(task.stationsIndex)
								.arg(upcomingSegmentIndex)
								.arg(task.unlockStationFrom)
								.arg(task.unlockStation));
						}
						task.status = LockStatus::Locking;
					}
					else {
						continue;
					}
				}
				if (task.status == LockStatus::Locking) {
					blocked = true;
					if (blockingAwaited && task.awaited > 0) {
						blockingAwaited->insert(task.awaited);
					}
					const QString logKey = makeLockBlockLogKey(task, waiterAgv);
					if (m_logger && !m_emittedLockBlockLogs.contains(logKey)) {
						m_logger->info(QString("锁阻塞：waiter=%1 awaited=%2 unlock=%3->%4 status=%5 stationIdx=%6 upcomingIdx=%7")
							.arg(waiterAgv)
							.arg(task.awaited)
							.arg(task.unlockStationFrom)
							.arg(task.unlockStation)
							.arg(lockStatusToString(task.status))
							.arg(task.stationsIndex)
							.arg(upcomingSegmentIndex));
						m_emittedLockBlockLogs.insert(logKey);
					}
				}
			}
			return blocked;
		}

		bool RouteDispatcher::detectMutualLock(int waiterAgv,
			const QSet<int>& blockingAwaited,
			QSet<int>& involvedAgvs) const {
			for (int awaited : blockingAwaited) {
				if (awaited <= 0) continue;
				auto it = m_lockListHash.find(awaited);
				if (it == m_lockListHash.end()) continue;
				const auto* list = it.value();
				for (const auto& task : list->tasks) {
					if (task.status == LockStatus::Locking && task.awaited == waiterAgv) {
						involvedAgvs.insert(waiterAgv);
						involvedAgvs.insert(awaited);
						return true;
					}
				}
			}
			return false;
		}

		void RouteDispatcher::clearLocksForAgv(int agvNumber) {
			if (m_lockListHash.contains(agvNumber)) {
				auto* list = m_lockListHash.value(agvNumber);
				if (list) {
					for (const auto& task : list->tasks) {
						clearLockBlockLogKey(task, list->agvNum);
					}
					list->tasks.clear();
				}
			}
			for (auto* list : m_lockLists) {
				if (!list) continue;
				for (auto& task : list->tasks) {
					if (task.awaited == agvNumber) {
						task.status = LockStatus::Finished;
						clearLockBlockLogKey(task, task.waiter);
					}
				}
			}
		}

		void RouteDispatcher::resetLocksForAgvs(const QSet<int>& agvs) {
			if (agvs.isEmpty()) return;
			for (int agv : agvs) {
				clearLocksForAgv(agv);
			}
		}

			QString RouteDispatcher::currentPlanGoal(int agvNumber) const {
				// 唯一目标来源：m_taskGoalCache（任务系统入口缓存）
				if (m_taskGoalCache.contains(agvNumber)) {
					return m_taskGoalCache.value(agvNumber);
				}
				return QString();
			}

		int RouteDispatcher::nextSegmentIndex(int agvNumber) const {
			return m_segmentCursor.value(agvNumber, 0);
		}

		void RouteDispatcher::advanceSegmentCursor(int agvNumber, int delta) {
			if (delta <= 0) return;
			const int current = m_segmentCursor.value(agvNumber, 0);
			m_segmentCursor.insert(agvNumber, current + delta);
		}

		void RouteDispatcher::resolveDeadlock(const QSet<int>& involvedAgvs) {
			if (involvedAgvs.isEmpty()) {
				return;
			}

			if (m_logger) {
				QStringList agvList;
				for (int agv : involvedAgvs) {
					agvList.append(QString::number(agv));
				}
				m_logger->warning(QString("Detected mutual wait among AGVs: %1. Re-planning.")
					.arg(agvList.join(", ")));
			}

			QList<PendingRequest> replanRequests;
			for (int agv : involvedAgvs) {
				// Cancel current vehicle task to align vehicle state with new plan
				if (m_vehicleService && !m_vehicleService->cancelAgvTask(agv)) {
					if (m_logger) {
						m_logger->warning(
							QString("Failed to cancel running task for AGV %1 before replanning")
							.arg(agv));
					}
				}
						const QString goal = currentPlanGoal(agv);
						auto* agvEntity =
							m_vehicleService ? m_vehicleService->getVehicle(agv) : nullptr;
						cancelPendingForAgv(agv, QStringLiteral("deadlock replan"));
						const QString start = resolveStartStation(agvEntity);
						if (start.isEmpty() || goal.isEmpty()) {
							continue;
						}
				replanRequests.push_back({ agv, start, goal, QString(), {} });
			}

			for (const auto& req : replanRequests) {
				m_pending.push_back(req);
			}

			if (!replanRequests.isEmpty()) {
				scheduleSolveAttempt();
			}
		}

		void RouteDispatcher::finalizeLocksForWaiter(int waiterAgv) {
			auto it = m_lockListHash.find(waiterAgv);
			if (it == m_lockListHash.end()) return;
			auto* list = it.value();
			for (auto& t : list->tasks) {
				if (t.status == LockStatus::UnLock) {
					t.status = LockStatus::Finished;
					clearLockBlockLogKey(t, waiterAgv);
				}
			}
		}

		QString RouteDispatcher::lockStatusToString(RouteDispatcher::LockStatus status) {
			switch (status) {
			case RouteDispatcher::LockStatus::Pending:
				return "Pending";
			case RouteDispatcher::LockStatus::Locking:
				return "Locking";
			case RouteDispatcher::LockStatus::UnLock:
				return "UnLock";
			case RouteDispatcher::LockStatus::Finished:
				return "Finished";
			default:
				return "Unknown";
			}
		}

		QString RouteDispatcher::makeLockBlockLogKey(const RouteDispatcher::LockTask& task, int waiterAgv) const {
			return QString("%1|%2|%3|%4|%5")
				.arg(waiterAgv)
				.arg(task.awaited)
				.arg(task.unlockStationFrom)
				.arg(task.unlockStation)
				.arg(task.stationsIndex);
		}

		void RouteDispatcher::clearLockBlockLogKey(const RouteDispatcher::LockTask& task, int waiterAgv) {
			m_emittedLockBlockLogs.remove(makeLockBlockLogKey(task, waiterAgv));
		}

		QSet<QString> RouteDispatcher::collectBlockAreasForPath(const QStringList& path) const {
			QSet<QString> areas;
			if (!m_mapService) {
				return areas;
			}
			for (const auto& station : path) {
				const QString areaId = m_mapService->getBlockAreaForStation(m_floor, station);
				if (!areaId.isEmpty()) {
					areas.insert(areaId);
				}
			}
			return areas;
		}

		void RouteDispatcher::reserveBlockAreas(int agvNumber, const QSet<QString>& areaIds) {
			if (areaIds.isEmpty()) {
				return;
			}
			for (const auto& areaId : areaIds) {
				m_blockAreaReservations.insert(areaId, agvNumber);
				m_agvReservedBlockAreas[agvNumber].insert(areaId);
			}
			if (m_logger) {
				QStringList items = areaIds.values();
				m_logger->debug(QString("BlockArea reserved by AGV %1: %2")
					.arg(agvNumber)
					.arg(items.join(", ")));
			}
		}

		void RouteDispatcher::releaseBlockAreasForAgv(int agvNumber) {
			auto it = m_agvReservedBlockAreas.find(agvNumber);
			if (it == m_agvReservedBlockAreas.end()) {
				return;
			}
			for (const auto& areaId : it.value()) {
				if (m_blockAreaReservations.value(areaId) == agvNumber) {
					m_blockAreaReservations.remove(areaId);
				}
			}
			m_agvReservedBlockAreas.erase(it);
			if (m_logger) {
				m_logger->debug(QString("BlockArea reservations cleared for AGV %1").arg(agvNumber));
			}
		}

		// ========== 拯救功能实现 ==========

			void RouteDispatcher::markAgvAsWaiting(int agvNumber, const QString& targetGoal) {
				if (targetGoal.isEmpty()) {
					return;
				}

			WaitingAgvInfo info;
			info.agvNumber = agvNumber;
			info.targetGoal = targetGoal;
			info.waitingSince = QDateTime::currentDateTime();
			m_waitingAgvs.insert(agvNumber, info);

				if (m_logger) {
					m_logger->info(QString("AGV %1 marked as waiting for goal %2")
						.arg(agvNumber).arg(targetGoal));
				}

				// 确保等待监控定时器运行，避免仅有等待车时不再检查
				if (!m_stateCheckTimer.isActive()) {
					m_stateCheckTimer.start();
				}
			}

		void RouteDispatcher::checkAndRescheduleWaitingAgvs(const QString& releasedStation) {
			Q_UNUSED(releasedStation);
			// 由持续监控逻辑处理
		}

			void RouteDispatcher::processWaitingAgvs() {
				if (m_waitingAgvs.isEmpty()) {
					return;
				}
	
			struct RescheduleItem {
				int agv;
				QString goal;
			};
			QList<RescheduleItem> toReschedule;

				for (auto it = m_waitingAgvs.begin(); it != m_waitingAgvs.end(); ) {
					const auto& info = it.value();
					const QString targetGoal = info.targetGoal;

					// 检查目标是否仍被其他任务占用（pending / planned / last goal）
					bool claimed = false;
					QStringList blockers;
					for (const auto& req : m_pending) {
						if (req.agvNumber != info.agvNumber && req.goal == targetGoal) {
							blockers.append(QStringLiteral("pending:AGV%1").arg(req.agvNumber));
							claimed = true;
							break;
						}
					}
					if (!claimed) {
						for (auto itSeg = m_plannedSegments.begin(); itSeg != m_plannedSegments.end() && !claimed; ++itSeg) {
							if (itSeg.key() == info.agvNumber) continue;
							const auto& queue = itSeg.value();
							if (!queue.isEmpty() && !queue.last().isEmpty() && queue.last().last() == targetGoal) {
								blockers.append(QStringLiteral("planned:AGV%1").arg(itSeg.key()));
								claimed = true;
								break;
							}
						}
					}
					if (!claimed) {
						for (auto itLast = m_lastSegmentGoal.begin(); itLast != m_lastSegmentGoal.end() && !claimed; ++itLast) {
							if (itLast.key() == info.agvNumber) continue;
							if (itLast.value() == targetGoal) {
								blockers.append(QStringLiteral("lastGoal:AGV%1").arg(itLast.key()));
								claimed = true;
								break;
							}
						}
					}

					if (m_logger) {
						if (claimed && !blockers.isEmpty()) {
							m_logger->info(QString("Waiting AGV %1 still blocked for goal %2 by [%3]")
								.arg(info.agvNumber)
								.arg(targetGoal)
								.arg(blockers.join(", ")));
						} else if (!claimed) {
							m_logger->info(QString("Waiting AGV %1 ready to reschedule goal %2")
								.arg(info.agvNumber)
								.arg(targetGoal));
						}
					}

					if (!claimed) {
						auto* agvEntity = m_vehicleService ? m_vehicleService->getVehicle(info.agvNumber) : nullptr;
						if (!agvEntity) {
							++it;  // 保持等待，下一轮再试
							continue;
						}
						QString start = resolveStartStation(agvEntity);
						if (start.isEmpty()) {
							if (m_logger) {
								m_logger->warning(QString("Waiting AGV %1 cannot resolve start, keep waiting for goal %2")
									.arg(info.agvNumber)
									.arg(targetGoal));
							}
							++it;  // 保持等待，避免丢失
							continue;
						}

						toReschedule.append({info.agvNumber, targetGoal});
						it = m_waitingAgvs.erase(it);
					} else {
						++it;
					}
				}

			if (toReschedule.isEmpty()) {
				return;
			}

			for (const auto& item : toReschedule) {
				auto* agvEntity = m_vehicleService ? m_vehicleService->getVehicle(item.agv) : nullptr;
				if (!agvEntity) continue;
				QString start = resolveStartStation(agvEntity);
				if (start.isEmpty()) continue;
				PendingRequest req{ item.agv, start, item.goal, m_agvOperations.value(item.agv), {}, QDateTime::currentMSecsSinceEpoch() };
				m_pending.push_back(std::move(req));
				if (m_logger) {
					m_logger->info(QString("Waiting AGV %1 rescheduled: %2 -> %3 (target free)")
						.arg(item.agv).arg(start).arg(item.goal));
				}
			}

			if (!toReschedule.isEmpty()) {
				scheduleSolveAttempt();
			}
		}

			void RouteDispatcher::slotCheckNavigationState() {
				processWaitingAgvs();  // 监控等待车状态变更
				if (m_dispatchedPending.isEmpty()) {
					if (m_waitingAgvs.isEmpty()) {
						m_stateCheckTimer.stop();
					}
					return;
				}

			const QDateTime now = QDateTime::currentDateTime();
			QList<int> toRetry;
			QList<int> toFail;

			for (auto it = m_dispatchedPending.begin(); it != m_dispatchedPending.end(); ) {
				auto& info = it.value();
				auto* agvEntity = m_vehicleService ? m_vehicleService->getVehicle(info.agvNumber) : nullptr;

				if (!agvEntity) {
					it = m_dispatchedPending.erase(it);
					continue;
				}

				// 检查是否已进入导航状态或有进度
				const bool isNavigating = (agvEntity->getNavStatus() == kernel::agv::NavStatus::Running);
				const QString currentStation = QString::fromStdString(agvEntity->getCurrentStation());
				const bool hasProgress = !currentStation.isEmpty() &&
										  info.segment.contains(currentStation) &&
										  currentStation != info.segment.first();

				if (isNavigating || hasProgress) {
					// 确认导航已开始，移除跟踪
					it = m_dispatchedPending.erase(it);
					continue;
				}

				// 检查超时
				const qint64 elapsed = info.dispatchedAt.msecsTo(now);
				if (elapsed > DispatchedSegmentInfo::TimeoutMs) {
					if (info.retryCount < DispatchedSegmentInfo::MaxRetries) {
						toRetry.append(info.agvNumber);
						info.retryCount++;
						info.dispatchedAt = now;
					} else {
						toFail.append(info.agvNumber);
						it = m_dispatchedPending.erase(it);
						continue;
					}
				}
				++it;
			}

			// 处理重试
			for (int agv : toRetry) {
				auto& info = m_dispatchedPending[agv];
				if (m_logger) {
					m_logger->warning(QString("Navigation timeout for AGV %1, retrying (%2/%3)")
						.arg(agv).arg(info.retryCount).arg(DispatchedSegmentInfo::MaxRetries));
				}

				std::vector<std::string> stations;
				for (const auto& s : info.segment) {
					stations.emplace_back(s.toStdString());
				}
				m_vehicleService->moveVehicleToStations(agv, stations, "");
			}

			// 处理失败（超过最大重试次数）→ 触发重新规划
			for (int agv : toFail) {
				if (m_logger) {
					m_logger->error(QString("Navigation failed for AGV %1 after max retries, triggering replan")
						.arg(agv));
				}

				auto* agvEntity = m_vehicleService ? m_vehicleService->getVehicle(agv) : nullptr;
				QString goal = currentPlanGoal(agv);
				cancelPendingForAgv(agv, QStringLiteral("nav failed max retries"));

				if (agvEntity && !goal.isEmpty()) {
					QString start = resolveStartStation(agvEntity);
					if (!start.isEmpty()) {
						m_pending.push_back({agv, start, goal, m_agvOperations.value(agv), {}});
						scheduleSolveAttempt();
					}
				}
			}
		}

		bool RouteDispatcher::rescheduleAgvTask(int agvNumber) {
			auto* agv = m_vehicleService ? m_vehicleService->getVehicle(agvNumber) : nullptr;
			if (!agv || !agv->isOnline()) {
				if (m_logger) {
					m_logger->warning(QString("rescheduleAgvTask: AGV %1 not found or offline")
						.arg(agvNumber));
				}
				return false;
			}

			// 检查是否在本楼层
			if (QString::fromStdString(agv->getCurrentMap()) != m_mapName) {
				if (m_logger) {
					m_logger->warning(QString("rescheduleAgvTask: AGV %1 not on this map")
						.arg(agvNumber));
				}
				return false;
			}

				// 唯一目标来源：m_taskGoalCache（任务系统入口缓存）
				QString goal;
				if (m_taskGoalCache.contains(agvNumber)) {
					goal = m_taskGoalCache.value(agvNumber);
				}

			if (goal.isEmpty()) {
				if (m_logger) {
					m_logger->warning(QString("rescheduleAgvTask: AGV %1 has no goal to reschedule")
						.arg(agvNumber));
				}
				return false;
			}

			// 获取起点
			StartResolution startInfo = resolveStartStationWithMeta(agv);
			QString start = startInfo.station;

			if (start.isEmpty()) {
				if (m_logger) {
					m_logger->warning(QString("rescheduleAgvTask: cannot resolve start for AGV %1")
						.arg(agvNumber));
				}
				return false;
			}

			// 加入待求解队列
			PendingRequest req{ agvNumber, start, goal, m_agvOperations.value(agvNumber), {}, QDateTime::currentMSecsSinceEpoch() };
			m_pending.push_back(std::move(req));

			// 正常冻结机制
			if (startInfo.usedTargetAsStart || startInfo.usedLastStation) {
				m_solveFreezeAgvs.insert(agvNumber);
			}

			if (m_logger) {
				m_logger->info(QString("rescheduleAgvTask: AGV %1 requeued %2 -> %3")
					.arg(agvNumber).arg(start).arg(goal));
			}

			scheduleSolveAttempt();
			return true;
		}

	}  // namespace dispatcher
}  // namespace application
